/*
 * AhaProtocol.cpp
 *
 *  Created on: 2 avr. 2011
 *      Author: eric
 */

#include "AhaProtocol.h"

void AhaProtocol::getAhaMessage(Can::CAN_MSG *canMsg,
		AhaProtocol::AHA_MSG *ahaMsg) {
	ahaMsg->CidPri = (canMsg->ID & AhaProtocol::CID_PRI_MASK) >> CID_PRI_SHIFT;
	ahaMsg->CidTyp = (canMsg->ID & AhaProtocol::CID_TYP_MASK) >> CID_TYP_SHIFT;
	ahaMsg->CidNodeSrc = (canMsg->ID & AhaProtocol::CID_NODE_SRC_MASK)
			>> CID_NODE_SRC_SHIFT;
	if (ahaMsg->CidTyp == CID_TYP_NV) {
		ahaMsg->CidDir = (canMsg->ID & AhaProtocol::CID_DIR_MASK)
				>> CID_DIR_SHIFT;
		ahaMsg->CidNvBnd = (canMsg->ID & AhaProtocol::CID_NV_BND_MASK)
				>> CID_NV_BND_SHIFT;
	} else {
		ahaMsg->CidNodeDst = (canMsg->ID & AhaProtocol::CID_NODE_DST_MASK)
				>> CID_NODE_DST_SHIFT;
		ahaMsg->CidEm = (canMsg->ID & AhaProtocol::CID_EM_MASK) >> CID_EM_SHIFT;
	}
	ahaMsg->nbytes = canMsg->nbytes;
	for (uint8_t i = 0; i < 8; i++) {
		if (i < canMsg->nbytes)
			ahaMsg->data[i] = canMsg->data[i];
		else
			ahaMsg->data[i] = 0;
	}
}

void AhaProtocol::setCanMessage(Can::CAN_MSG *canMsg,
		AhaProtocol::AHA_MSG *ahaMsg) {
	if (ahaMsg->CidTyp == CID_TYP_NV) {
		canMsg->ID = ((uint32_t) ahaMsg->CidPri << CID_PRI_SHIFT)
				+ ((uint32_t) ahaMsg->CidTyp << CID_TYP_SHIFT)
				+ ((uint32_t) ahaMsg->CidNodeSrc << CID_NODE_SRC_SHIFT)
				+ ((uint32_t) ahaMsg->CidDir << CID_DIR_SHIFT)
				+ ((uint32_t) ahaMsg->CidNvBnd);
	} else {
		canMsg->ID = ((uint32_t) ahaMsg->CidPri << CID_PRI_SHIFT)
				+ ((uint32_t) ahaMsg->CidTyp << CID_TYP_SHIFT)
				+ ((uint32_t) ahaMsg->CidNodeSrc << CID_NODE_SRC_SHIFT)
				+ ((uint32_t) ahaMsg->CidNodeDst << CID_NODE_DST_SHIFT)
				+ ((uint32_t) ahaMsg->CidEm);
	}
	canMsg->rtr = false;
	canMsg->nbytes = ahaMsg->nbytes;
	for (uint8_t i = 0; i < canMsg->nbytes; i++) {
		canMsg->data[i] = ahaMsg->data[i];
	}
}

void AhaProtocol::sendEMNeedCfg(AhaProtocol::AHA_MSG *ahaMsg, uint16_t nodeAdr){
	ahaMsg->CidPri = CID_PRI_HIGH;
	ahaMsg->CidTyp = CID_TYP_EM;
	ahaMsg->CidEm = CID_EM_NEED_CFG;
	ahaMsg->CidNodeSrc = nodeAdr & CID_11b_MASK;
	ahaMsg->CidNodeDst = AhaProtocol::MASTER_CFG_NODE;
	ahaMsg->nbytes = 0;
}


void AhaProtocol::sendEMNeedId(AhaProtocol::AHA_MSG *ahaMsg, byte *nodeID) {
	ahaMsg->CidPri = CID_PRI_HIGH;
	ahaMsg->CidTyp = CID_TYP_EM;
	ahaMsg->CidEm = CID_EM_NEED_ID;
	ahaMsg->CidNodeSrc = AhaProtocol::BROADCAST_NODE;
	ahaMsg->CidNodeDst = AhaProtocol::MASTER_CFG_NODE;
	ahaMsg->nbytes = 6;
	ahaMsg->data[0] = nodeID[0];
	ahaMsg->data[1] = nodeID[1];
	ahaMsg->data[2] = nodeID[2];
	ahaMsg->data[3] = nodeID[3];
	ahaMsg->data[4] = nodeID[4];
	ahaMsg->data[5] = nodeID[5];
}

